#include <ros/ros.h>

#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<sensor_msgs/NavSatFix.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Point.h>
#include<std_msgs/UInt64.h>
#include <std_msgs/Bool.h>
#include<sensor_msgs/PointCloud2.h>
#include <mavros_msgs/Altitude.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>
#include <mavros_msgs/RCIn.h>

#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include <fastdds/dds/publisher/Publisher.hpp>

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"
//接收ros发布的topic,后dds再发布topic

#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include <string>
#include <vector>
#include <sstream>
#include <iostream>

ros::Subscriber tag_flag_sub;
//声明fastdds发布、ros的订阅类
// SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddspub_pose=nullptr;

ros::Subscriber rossub_state, rossub_pose, rosrcin_sub;
ros::Subscriber rossub_human, rossub_car, rossub_oil, rossub_weapon, rossub_bigarmor, rossub_shatanche, rossub_chuzhi;
ros::Publisher rospub_pose, rospub_cloud, rospub_target,rospub_planner;

// SDPltPublisher<PointCloud2PubSubType, PointCloud2>* ddspub_pointcloud=nullptr;
// SDPltPublisher<PointCloud2PubSubType, PointCloud2>* ddspub_planner=nullptr;

ros::Subscriber rossub_pointcloud, rossub_planner;
// SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddspub_target=nullptr;
// SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddspub_chuzhi=nullptr;

RegisterMSN *reg_pub = new RegisterMSN();

struct Point3D {
    float x, y, z;
 
    // 构造函数，方便初始化
    Point3D(float x = 0, float y = 0, float z = 0) : x(x), y(y), z(z) {}
};

std::vector<Point3D> vehicle_offset = {

    {0.0, 0.0, 0.0},
    {0.0, 5.8, 0.0},
    {-2.5, 0.0, -0.5}, 
    {-2.5, 5.8, 0.5},

    {-5.0, 0.0, 0.0},
    {-5.0, 6.0, 0.0},

    {-4.0, 0.0, 0.0},
    {-4.0, 2.0, 0.0},
    {-4.0, 4.0, 0.0},

    {4.0, 2.0, 0.0},
    {2.0, 2.0, 0.0},
    {0.0, 2.0, 0.0},
    //10架室外侦察巡逻
    {18.0, 4.0, 0.0},
    {16.0, 4.0, 0.0},
    {14.0, 4.0, 0.0},
    {12.0, 4.0, 0.0},
    {10.0, 4.0, 0.0},
    {8.0, 4.0, 0.0},
    {6.0, 4.0, 0.0},
    {4.0, 4.0, 0.0},
    {2.0, 4.0, 0.0},
    {0.0, 4.0, 0.0}
};

Point3D self_offset;
int drone_id;
double position_offset_x = 0.0;
double position_offset_y = 0.0;
double position_offset_z = 0.0;
double point_x = 0.0;
double point_y = 0.0;
double point_z = 0.0;

int channel7_value = 1050;
int channel6_value = 1050;
double last_pub_pc_time = 0.0;
double last_pub_pose_time = 0.0;

double last_pub_human_time = 0.0;
double last_pub_car_time = 0.0;
double last_pub_oil_time = 0.0;
double last_pub_weapon_time = 0.0;
double last_pub_bigarmor_time = 0.0;
double last_pub_shatanche_time = 0.0;
double last_pub_planner_time = 0.0;
double last_pub_chuzhi_time = 0.0;
int sequence = 0;

int pub_human_cnt = 5;
int pub_car_cnt = 5;
int pub_oil_cnt = 5;
int pub_weapon_cnt = 5;
int pub_bigarmor_cnt = 5;
int pub_shatanche_cnt = 5;

std::vector<geometry_msgs::Point> human_saved;
std::vector<geometry_msgs::Point> car_saved;
std::vector<geometry_msgs::Point> oil_saved;
std::vector<geometry_msgs::Point> weapon_saved;
std::vector<geometry_msgs::Point> bigarmor_saved;
std::vector<geometry_msgs::Point> shatanche_saved;

geometry_msgs::Point gps_point;

bool has_odom = false;
mavros_msgs::State current_state;

pcl::PointCloud<pcl::PointXYZ> saved_history_cloud;
int history_pub_cnt = 4;

void rosstateloopdata(const mavros_msgs::State::ConstPtr &state)
{
    current_state = *state;
}


//ros (/zhou/new_odom) 回调函数
void rosposeloopdata(const nav_msgs::Odometry::ConstPtr &odom){
    has_odom = true;
    // if(channel7_value>1800)
    // {
    //     return;
    // }
    if(ros::Time::now().toSec()-last_pub_pose_time<0.2)
    {
        return;
    }

    geometry_msgs::PoseStamped msg_pub;


    std::string set_frame_id = std::to_string(drone_id);
    if(channel6_value>1700)
    {
        point_x = -99;
        point_y = -99;
        point_z = -10;
    }
    else
    {
        point_x = odom->pose.pose.position.x+position_offset_x;
        point_y = odom->pose.pose.position.y+position_offset_y;
        point_z = odom->pose.pose.position.z+position_offset_z;
    }
    
    //赋值
    msg_pub.header.seq = sequence;
    sequence++;
    msg_pub.header.frame_id = set_frame_id;
    msg_pub.pose.position.x = point_x;
    msg_pub.pose.position.y = point_y;
    msg_pub.pose.position.z = point_z;
    msg_pub.pose.orientation.x = odom->pose.pose.orientation.x;
    msg_pub.pose.orientation.y = odom->pose.pose.orientation.y;
    msg_pub.pose.orientation.z = odom->pose.pose.orientation.z;
    msg_pub.pose.orientation.w = odom->pose.pose.orientation.w;
    rospub_pose.publish(msg_pub);

    
    last_pub_pose_time = ros::Time::now().toSec();
    
}

void rcin_callback(const mavros_msgs::RCIn::ConstPtr &rcvalue)
{
    channel6_value = rcvalue->channels[5];
    channel7_value = rcvalue->channels[6];

}

void human_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }
    
    if(ros::Time::now().toSec()-last_pub_human_time<0.3)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "human";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        human_saved.push_back(*msg);
        pub_human_cnt=0;
    }
    else
    {
        if(pub_human_cnt<5)
        {
            for(int i=0;i<human_saved.size();i++)
            {
                msg_pub.pose.position.x = human_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = human_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = human_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_human_time = ros::Time::now().toSec();
            pub_human_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_human_time = ros::Time::now().toSec();
            human_saved.clear();
        }
    }

}
void car_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }
   
    if(ros::Time::now().toSec()-last_pub_car_time<0.2)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "car";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        car_saved.push_back(*msg);
        pub_car_cnt=0;
    }
    else
    {
        if(pub_car_cnt<5)
        {
            for(int i=0;i<car_saved.size();i++)
            {
                msg_pub.pose.position.x = car_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = car_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = car_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_car_time = ros::Time::now().toSec();
            pub_car_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_car_time = ros::Time::now().toSec();
            car_saved.clear();
        }
    }

}
void oil_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }
  
    if(ros::Time::now().toSec()-last_pub_oil_time<0.2)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "oil";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        oil_saved.push_back(*msg);
        pub_oil_cnt=0;
    }
    else
    {
        if(pub_oil_cnt<5)
        {
            for(int i=0;i<oil_saved.size();i++)
            {
                msg_pub.pose.position.x = oil_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = oil_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = oil_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_oil_time = ros::Time::now().toSec();
            pub_oil_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_oil_time = ros::Time::now().toSec();
            oil_saved.clear();
        }
    }

}
void weapon_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }
   
    if(ros::Time::now().toSec()-last_pub_weapon_time<0.2)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "weapon";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        weapon_saved.push_back(*msg);
        pub_weapon_cnt=0;
    }
    else
    {
        if(pub_weapon_cnt<5)
        {
            for(int i=0;i<weapon_saved.size();i++)
            {
                msg_pub.pose.position.x = weapon_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = weapon_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = weapon_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_weapon_time = ros::Time::now().toSec();
            pub_weapon_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_weapon_time = ros::Time::now().toSec();
            weapon_saved.clear();
        }
    }

}
void bigarmor_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }
  
    if(ros::Time::now().toSec()-last_pub_bigarmor_time<0.2)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "bigarmor";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        bigarmor_saved.push_back(*msg);
        pub_bigarmor_cnt=0;
    }
    else
    {
        if(pub_bigarmor_cnt<5)
        {
            for(int i=0;i<bigarmor_saved.size();i++)
            {
                msg_pub.pose.position.x = bigarmor_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = bigarmor_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = bigarmor_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_bigarmor_time = ros::Time::now().toSec();
            pub_bigarmor_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_bigarmor_time = ros::Time::now().toSec();
            bigarmor_saved.clear();
        }
    }

}
void shatanche_callback(const geometry_msgs::Point::ConstPtr &msg)
{
    if(!has_odom)
    {
        return;
    }

    if(ros::Time::now().toSec()-last_pub_shatanche_time<0.2)
    {
        return;
    }
    geometry_msgs::PoseStamped msg_pub;

    std::string set_frame_id = "shatanche";
    msg_pub.header.frame_id = set_frame_id;

    if(channel7_value>1800)
    {
        shatanche_saved.push_back(*msg);
        pub_shatanche_cnt=0;
    }
    else
    {
        if(pub_shatanche_cnt<5)
        {
            for(int i=0;i<shatanche_saved.size();i++)
            {
                msg_pub.pose.position.x = shatanche_saved[i].x+position_offset_x;
                msg_pub.pose.position.y = shatanche_saved[i].y+position_offset_y;
                msg_pub.pose.position.z = shatanche_saved[i].z+position_offset_z;
                rospub_target.publish(msg_pub);
            }
            last_pub_shatanche_time = ros::Time::now().toSec();
            pub_shatanche_cnt++;
        }
        else
        {
            msg_pub.pose.position.x = msg->x+position_offset_x;
            msg_pub.pose.position.y = msg->y+position_offset_y;
            msg_pub.pose.position.z = msg->z+position_offset_z;
            rospub_target.publish(msg_pub);
            last_pub_shatanche_time = ros::Time::now().toSec();
            shatanche_saved.clear();
        }
    }

    

}
void planner_callback(const visualization_msgs::Marker::ConstPtr &Marker_points)
{
    if(!has_odom||channel7_value>1800)
    {
        return;
    }
  
    if(ros::Time::now().toSec()-last_pub_planner_time<0.2)
    {
        return;
    }
    // 创建新的Marker并复制头信息和属性
    visualization_msgs::Marker marker_pub;
    marker_pub.header = Marker_points->header;
    marker_pub.header.frame_id = "map";
    marker_pub.ns = Marker_points->ns;
    marker_pub.id = Marker_points->id;
    marker_pub.type = Marker_points->type;
    marker_pub.action = Marker_points->action;
    marker_pub.scale = Marker_points->scale;
    marker_pub.color = Marker_points->color;
    marker_pub.lifetime = Marker_points->lifetime;
    marker_pub.frame_locked = Marker_points->frame_locked;
    marker_pub.points = Marker_points->points;
    for(int i = 0;i<Marker_points->points.size();i++)
    {
        marker_pub.points[i].x = Marker_points->points[i].x+ position_offset_x;
        marker_pub.points[i].y = Marker_points->points[i].y+ position_offset_y;
        marker_pub.points[i].z = Marker_points->points[i].z+ position_offset_z;

    }
    rospub_planner.publish(marker_pub);
    last_pub_planner_time = ros::Time::now().toSec();

}

void pc_callback(const sensor_msgs::PointCloud2::ConstPtr &points)
{
    if(!has_odom)
    {
        return;
    }
    if(ros::Time::now().toSec()-last_pub_pc_time<0.5)
    {
        return;
    }
    //receive cloud message
    pcl::PointCloud<pcl::PointXYZ> latest_cloud1;
    pcl::fromROSMsg(*points, latest_cloud1);
    if(channel7_value>1800)
    {
        saved_history_cloud += latest_cloud1;
        saved_history_cloud.width  = saved_history_cloud.points.size();
        saved_history_cloud.height = 1;
        if (saved_history_cloud.points.size() != saved_history_cloud.width * saved_history_cloud.height) {
            std::cout<<"====>error points111.."<<std::endl;
        }
        
        //sample pointcloud
        pcl::PCLPointCloud2::Ptr cloud_in (new pcl::PCLPointCloud2 ());
        pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
        toPCLPointCloud2(saved_history_cloud,*cloud_in);
        pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
        sor.setInputCloud (cloud_in);
        sor.setLeafSize(0.2,0.2,0.2);
        sor.filter (*cloud_filtered);
        fromPCLPointCloud2(*cloud_filtered,saved_history_cloud);
        saved_history_cloud.width = saved_history_cloud.points.size();
        saved_history_cloud.height = 1;
        std::cout << "saving cloud,wait for communication regain...." << std::endl;
        history_pub_cnt = 0;
    }
    else
    {
        if(history_pub_cnt<4)
        {
            if(history_pub_cnt==0)
            {
                for(int i=0;i<saved_history_cloud.points.size();i++)
                {
                    saved_history_cloud.points[i].x += position_offset_x;
                    saved_history_cloud.points[i].y += position_offset_y;
                    saved_history_cloud.points[i].z += position_offset_z;
                }
            }
            saved_history_cloud.width = saved_history_cloud.points.size();
            saved_history_cloud.height = 1;
            sensor_msgs::PointCloud2 cloud_pub;
            pcl::toROSMsg(saved_history_cloud,cloud_pub);
            cloud_pub.header.frame_id = "map";
            rospub_cloud.publish(cloud_pub);
            std::cout << "publish history point cloud...." << std::endl;
            history_pub_cnt++;
            last_pub_pc_time = ros::Time::now().toSec();
        }
        else
        {
            if (latest_cloud1.points.size() == 0)
            {   
                std::cout << "no cloud points...." << std::endl;
                return;
            } 
            for(int i=0;i<latest_cloud1.points.size();i++)
            {
                latest_cloud1.points[i].x += position_offset_x;
                latest_cloud1.points[i].y += position_offset_y;
                latest_cloud1.points[i].z += position_offset_z;

            }
            latest_cloud1.width = latest_cloud1.points.size();
            latest_cloud1.height = 1;
            sensor_msgs::PointCloud2 cloud_pub;
            pcl::toROSMsg(latest_cloud1,cloud_pub);
            cloud_pub.header.frame_id = "map";
            rospub_cloud.publish(cloud_pub);

            last_pub_pc_time = ros::Time::now().toSec();
        }

    }


    
}



int main(int argc, char* argv[]){
    ros::init(argc, argv, "SDPltDDSPosepub");
    ros::NodeHandle nodeHandle; 
    reg_pub->detect_serial_number();

    // 设置循环的频率
    ros::Rate loop_rate(200);

    nodeHandle.getParam("drone_id",drone_id);

    self_offset = vehicle_offset[drone_id-1];
    position_offset_x = self_offset.x;
    position_offset_y = self_offset.y;
    position_offset_z = self_offset.z;



    //先接收ros发布的topic,后dds再发布topic
    rossub_state = nodeHandle.subscribe<mavros_msgs::State>("/mavros/state",10,rosstateloopdata);
    rossub_pose = nodeHandle.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom",10,rosposeloopdata);
    rosrcin_sub = nodeHandle.subscribe<mavros_msgs::RCIn>("/mavros/rc/in",10,rcin_callback);

    rossub_planner = nodeHandle.subscribe<visualization_msgs::Marker>("/ego_planner_node/optimal_list", 10, planner_callback);
  
    rossub_pointcloud = nodeHandle.subscribe<sensor_msgs::PointCloud2>("/cloud_registered", 10, pc_callback);
 

    rossub_human = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_human",10,human_callback);
    rossub_car = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_car",10,car_callback);
    rossub_oil = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_oil",10,oil_callback);
    rossub_weapon = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_weapon",10,weapon_callback);
    rossub_bigarmor = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_bigarmor",10,bigarmor_callback);
    rossub_shatanche = nodeHandle.subscribe<geometry_msgs::Point>("/zhou/pose_shatanche",10,shatanche_callback);

    rospub_pose = nodeHandle.advertise<geometry_msgs::PoseStamped>("/zhou/pose_all_drone", 10);
    rospub_planner = nodeHandle.advertise<visualization_msgs::Marker>("/zhou/planner_registerd", 10);
    rospub_cloud = nodeHandle.advertise<sensor_msgs::PointCloud2>("/zhou/pointcloud_registerd", 10);
    rospub_target = nodeHandle.advertise<geometry_msgs::PoseStamped>("/zhou/target_registerd", 10);

    
    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }
    // ros::spin();


    nodeHandle.shutdown();
    return 0;
}

